#ifndef ODE_CONTROLLED_SIMULATOR_H
#define ODE_CONTROLLED_SIMULATOR_H

#include "Controller.h"
#include "ODERobot.h"

struct ControlledRobotSimulator
{
  //settings
  Robot* robot;
  ODERobot* oderobot;
  RobotController* controller;
  Real controlTimeStep;

  //state
  Real curTime;
  Real nextControlTime;
  RobotCommand command;
  RobotSensors sensors;

  ControlledRobotSimulator();
  void Init(Robot* robot,ODERobot* oderobot,RobotController* controller=NULL);
  void Step(Real dt);
  void UpdateRobot();

  void SimulateSensors();
  void GetCommandedConfig(Config& q);
  void GetCommandedVelocity(Config& dq);
  void GetSensedConfig(Config& q);
  void GetSensedVelocity(Config& dq);
  RobotCommand* GetCommands() { return &command; }

  bool ReadState(File& f);
  bool WriteState(File& f) const;
};

#endif
